#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/time.h>  // gettimeofday
#include <sys/timeb.h> // ftime
#include <time.h>      // time clock_gettime
#include "testnet.h"
#include "in_temp.h"
#include "../can/can.h"
#include "../422/libaxidma.h"          // Local definitions
#include "../422/util.h"               // Miscellaneous utilities
#include "../422/conversion.h"        // Convert bytes to MiBs
#include "../422/axidma_ioctl.h"



extern struct dma_transfer trans_gtx;
extern axidma_dev_t axidma_dev;


#define DEBUG    0

#if DEBUG
    #define DBG_MSG(fmt, args...)    fprintf(stdout, fmt, ##args)
#else
    #define DBG_MSG(fmt, args...)
#endif

static int sockfd, udp_ready = 0;
static struct sockaddr_in serv_addr;
static int len = sizeof(serv_addr);


int count_dg = 0, count_jj = 0, count_gps = 0, count_bds = 0, count_acs = 0, count_orbit = 0, count_tlegps = 0, count_tlebds = 0, count_rs = 0, count_sar = 0, count_tx2yc = 0, count_udpsd = 0;//udp传输TX2应答种类计数
int change_422 = 0;//RS422通道切换标志变量
int mode_flag = 0;//AI工作模式标志

extern int indirect_end, gps_end, bds_end, tlegps_end, tlebds_end, acs_end, orbit_end, dma422_end, flag_orbit;
extern unsigned char indirect_data[];
extern unsigned char Recv_pcm0[];//RS422通道A接收数据
extern unsigned char Recv_pcm1[];//RS422通道A接收数据
extern unsigned char senddata_gb[];
extern unsigned char gps_recvdata[];//437字节
extern unsigned char bds_recvdata[];
extern unsigned char tlegps_recvdata[];
extern unsigned char tlebds_recvdata[];
extern unsigned char acs_recvdata[];
extern unsigned char orbit_recvdata[];
extern struct dma_transfer trans_gtx;
extern int gps_length, bds_length, tlegps_length, tlebds_length, acs_length, orbit_length, gps_recvdlc, bds_recvdlc, tlegps_recvdlc, tlebds_recvdlc, acs_recvdlc, orbit_recvdlc;
extern unsigned char senddata_gb[];
extern unsigned char timeout_flag;

unsigned char orbit_send[ORBIT_NUM] = {0};//定轨数据包轮询指令udp发送数据
unsigned char gps_send[GPS_NUM] = {0};//GPS数据包udp发送数据
unsigned char bds_send[BDS_NUM] = {0};//BDS数据包udp发送数据
unsigned char tlegps_send[TLE_NUM] = {0};//GNSS星历数据包udp发送数据
unsigned char tlebds_send[TLE_NUM] = {0};//GNSS星历数据包udp发送数据
unsigned char acs_send[ACS_NUM] = {0};//ACS数据包udp发送数据
unsigned char acsorbit_send[ACS_ORBIT_NUM] = {0};//ACS数据包udp发送数据
unsigned char indirect_send[INDIRECT_NUM] = {0};//间接指令数据包udp发送数据
unsigned char indirect_recv[INDIRECT_NUM] = {0};//间接指令数据包udp接收数据
unsigned char dma422_send[UDP422_NUM] = {0};//间接指令数据包udp发送数据
unsigned char dma422_recv[UDP422_NUM] = {0};//间接指令数据包udp发送数据
unsigned char udp_recv[UDP_NUM] = {0};//udp监听数据缓冲数组
unsigned char gtxbuffer[994] = {0};//gtx发送数据1+990+2+1单字节和
unsigned char tx2_yc[TX2_YC] = {170, 170, 170, 170, 170, 170, 170, 170, 170, 170};//TX2i遥测数据
unsigned char tx2_sar[TX2_SAR+4] = {};//SAR载荷处理图像回传数据

/* 测试变量  */
//1s向文件中写入一次广播数据
extern int test_pcm, test_pcm_1, count_pcm, count_gtxsd;
extern unsigned char tx2_state;
extern int fd_a, fd_b;
const char *filesar1 = "./sar.txt";
const char *filegps1 = "./gps0.txt";
const char *filebds1 = "./bds0.txt";
const char *filetlegps1 = "./tlegps0.txt";
const char *filetlebds1 = "./tlebds0.txt";
const char *fileacs2 = "./acs1.txt";
const char *filegps2 = "./gps1.txt";
const char *filebds2 = "./bds1.txt";
const char *filetlegps2 = "./tlegps1.txt";
const char *filetlebds2 = "./tlebds1.txt";

extern int acs_txt, gnss_txt, tle_txt, text_sar;//测试变量
int flag_reset=0, count_udp=0, count_udp1=0, flag_textsar=0, count_sars=0, count_sarss=0;//测试Zynq7045复位
int test_flag=0, test_flag1=0;
long time_1, time_2;//时间戳测试
extern int test_id, test_id_1;




/*************************
 *
 *
 *************************/

//时间戳 纳秒
long get_system_time_nanosecond()
{
    struct timespec timestamp = {};
    if (0 == clock_gettime(CLOCK_REALTIME, &timestamp))
        return timestamp.tv_sec * 1000000000 + timestamp.tv_nsec;
    else
        return 0;
}

//时间戳 微秒
long get_system_time_microsecond()
{
    struct timeval timestamp = {};
    if (0 == gettimeofday(&timestamp, NULL))
        return timestamp.tv_sec * 1000000 + timestamp.tv_usec;
    else
        return 0;
}

//时间戳 毫秒
long get_system_time_millisecond()
{
    struct timeb timestamp = {};

    if (0 == ftime(&timestamp))
        return timestamp.time * 1000 + timestamp.millitm;
    else
        return 0;
}

//时间戳 秒
long get_system_time_second()
{
    return time(NULL);
}




/* GPS接收436字节数据，sum和计算*/
unsigned char gps_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<gps_recvdlc-1; i++)
	{
		sum += gps_recvdata[i];
	}

	return sum;
}

/* BDS接收436字节数据，sum和计算*/
unsigned char bds_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<bds_recvdlc-1; i++)
	{
		sum += bds_recvdata[i];
	}

	return sum;
}

/* ACS接收245字节数据，sum和计算*/
unsigned char acs_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<acs_recvdlc-1; i++)
	{
		sum += acs_recvdata[i];
	}

	return sum;
}

/* ACS_orbit接收56字节数据，sum和计算*/
unsigned char orbit_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<orbit_recvdlc-1; i++)
	{
		sum += orbit_recvdata[i];
	}

	return sum;
}

/* tle接收GPS202字节数据，sum和计算 */
unsigned char tlegps_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<tlegps_recvdlc-1; i++)
	{
		sum += tlegps_recvdata[i];
	}

	return sum;
}

/* ACS接收245字节数据，sum和计算*/
unsigned char tlebds_sendsum()
{
	unsigned char sum = 0;

	for(int i=0; i<tlebds_recvdlc-1; i++)
	{
		sum += tlebds_recvdata[i];
	}

	return sum;
}

/* UDP接收数据累加和计算 */
unsigned char udp_recvsum( int udp_fdt )
{
	int i;
	unsigned char sum = 0;

	if( udp_fdt == 0x08 )
	{
		for(i=0; i < 68; i++)
		{
		   sum += udp_recv[i];
		}
	}
	else if( udp_fdt == 0x09 )
	{
		for(i=0; i < 10; i++)
		{
		   sum += udp_recv[i];
		}
	}
	else if( udp_fdt == 0x0A )
	{
		for(i=0; i < 998; i++)
		{
		   sum += udp_recv[i];
		}
	}
	else if( udp_fdt == 0x0B )
	{
		for(i=0; i < 16; i++)
		{
		   sum += udp_recv[i];
		}
	}

	return sum;
}

/* 轮询精密定轨数据指令,udp打包*/
void package_orbit()
{
    static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	orbit_send[0] = 0x01;
	orbit_send[1] = 0;
	orbit_send[2] = 0x04;
	orbit_send[3] = count >> 8;
	orbit_send[4] = count;
	orbit_send[5] = 0x01;

	orbit_send[6] = 0xAA;
	orbit_send[7] = 0xAA;
	orbit_send[8] = 0xAA;
	orbit_send[9] = 0xAA;

	orbit_send[10] = 0;
	for(int j=0; j<10; j++)
	{
		orbit_send[10] += orbit_send[j];
	}
}

/* 间接指令数据udp打包*/
void package_indiret()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	indirect_send[0] = 0x01;
	indirect_send[1] = 0;
	indirect_send[2] = 0x04;
	indirect_send[3] = count >> 8;
	indirect_send[4] = count;
	indirect_send[5] = 0x02;

	for(int i=0; i<INDIRECT_NUM-7; i++)
	{
		indirect_send[6+i] = indirect_data[i];
	}

	indirect_send[INDIRECT_NUM-1] = 0;
	for(int j=0; j<(INDIRECT_NUM-1); j++)
	{
		indirect_send[INDIRECT_NUM-1] += indirect_send[j];
	}
}

/* GPS数据udp打包*/
void package_gps()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	gps_send[0] = 0x01;
	gps_send[1] = (GPS_NUM-7) >> 8;
	gps_send[2] = (GPS_NUM-7);
	gps_send[3] = count >> 8;
	gps_send[4] = count;
	gps_send[5] = 0x03;

	memcpy(gps_send+6, gps_recvdata+7, 435);//memcpy（目标地址，源地址，数据长度）
	gps_send[GPS_NUM-1] = 0;

	for(int i=0; i<(GPS_NUM-1); i++)
	{
		gps_send[GPS_NUM-1] += gps_send[i];
	}
}

/* BDS数据udp打包*/
void package_bds()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	bds_send[0] = 0x01;
	bds_send[1] = (BDS_NUM-7) >> 8;
	bds_send[2] = (BDS_NUM-7);
	bds_send[3] = count >> 8;
	bds_send[4] = count;
	bds_send[5] = 0x04;



	memcpy(bds_send+6, bds_recvdata+7, 435);//memcpy（目标地址，源地址，数据长度）
	bds_send[BDS_NUM-1] = 0;

	for(int i=0; i<(BDS_NUM-1); i++)
	{
		bds_send[BDS_NUM-1] += bds_send[i];
	}
}


/* ACS数据udp打包 */
void package_acs()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	acs_send[0] = 0x01;
	acs_send[1] = (ACS_NUM-7) >> 8;
	acs_send[2] = (ACS_NUM-7);
	acs_send[3] = count >> 8;
	acs_send[4] = count;
	acs_send[5] = 0x05;

	memcpy(acs_send+6, acs_recvdata+7, 244);//memcpy（目标地址，源地址，数据长度）
	acs_send[ACS_NUM-1] = 0;

	for(int i=0; i<(ACS_NUM-1); i++)
	{
		acs_send[ACS_NUM-1] += acs_send[i];
	}
}


/* ACS轨道数据udp打包 */
void package_acsorbit()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	acsorbit_send[0] = 0x01;
	acsorbit_send[1] = (ACS_ORBIT_NUM-7) >> 8;
	acsorbit_send[2] = (ACS_ORBIT_NUM-7);
	acsorbit_send[3] = count >> 8;
	acsorbit_send[4] = count;
	acsorbit_send[5] = 0x09;

	memcpy(acsorbit_send+6, orbit_recvdata+7, 55);//memcpy（目标地址，源地址，数据长度）
	acsorbit_send[ACS_ORBIT_NUM-1] = 0;

	for(int i=0; i<(ACS_ORBIT_NUM-1); i++)
	{
		acsorbit_send[ACS_ORBIT_NUM-1] += acsorbit_send[i];
	}
}


/* GNSS星历GPS数据udp打包 */
void package_tlegps()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	tlegps_send[0] = 0x01;
	tlegps_send[1] = (TLE_NUM-7) >> 8;
	tlegps_send[2] = (TLE_NUM-7);
	tlegps_send[3] = count >> 8;
	tlegps_send[4] = count;
	tlegps_send[5] = 0x06;

	memcpy(tlegps_send+6, tlegps_recvdata+7, 202);//memcpy（目标地址，源地址，数据长度）
	tlegps_send[TLE_NUM-1] = 0;

	for(int i=0; i<(TLE_NUM-1); i++)
	{
		tlegps_send[TLE_NUM-1] += tlegps_send[i];
	}
}

/* GNSS星历BDS数据udp打包 */
void package_tlebds()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	tlebds_send[0] = 0x01;
	tlebds_send[1] = (TLE_NUM-7) >> 8;
	tlebds_send[2] = (TLE_NUM-7);
	tlebds_send[3] = count >> 8;
	tlebds_send[4] = count;
	tlebds_send[5] = 0x07;

	memcpy(tlebds_send+6, tlebds_recvdata+7, 202);//memcpy（目标地址，源地址，数据长度）
	tlebds_send[TLE_NUM-1] = 0;

	for(int i=0; i<(TLE_NUM-1); i++)
	{
		tlebds_send[TLE_NUM-1] += tlebds_send[i];
	}
}


/* 高速上行数据udp打包*/
void package_dma422()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	dma422_send[0] = 0x01;
	dma422_send[1] = (UDP422_NUM-7) >> 8;
	dma422_send[2] = UDP422_NUM-7;
	dma422_send[3] = count >> 8;
	dma422_send[4] = count;
	dma422_send[5] = 0x08;

	for(int i=0; i<UDP422_NUM-7; i++)
	{
		dma422_send[6+i] = Recv_pcm0[i+26];
	}

	dma422_send[UDP422_NUM-1] = 0;

	for(int j=0; j<UDP422_NUM-1; j++)
	{
		dma422_send[UDP422_NUM-1] += dma422_send[j];
	}
}

/* RS422通道B数据udp打包*/
void package_dma422_1()
{
	static int count = 0;

	if( count < 65535 )
	  count++;
	else
		count = 1;

	dma422_send[0] = 0x01;
	dma422_send[1] = (UDP422_NUM-7) >> 8;
	dma422_send[2] = (UDP422_NUM-7);
	dma422_send[3] = count >> 8;
	dma422_send[4] = count;
	dma422_send[5] = 0x08;

	for(int i=0; i<UDP422_NUM-7; i++)
	{
		dma422_send[6+i] = Recv_pcm1[i+26];
	}

	dma422_send[UDP422_NUM-1] = 0;

	for(int j=0; j<UDP422_NUM-1; j++)
	{
		dma422_send[UDP422_NUM-1] += dma422_send[j];
	}
}


/* TX2i开机、关机控制*/
void ctl_tx2(char val)
{
	 int valuefd, exportfd, directionfd;
	 exportfd = open("/sys/class/gpio/export", O_WRONLY);
	 if (exportfd < 0)
	 {
		 DBG_MSG("Cannot open GPIO to export it\n");
		 return;
	 }
	 write(exportfd, "960", 4);
	 close(exportfd);
	 directionfd = open("/sys/class/gpio/gpio960/direction", O_RDWR);
	 if (directionfd < 0)
	 {
		 DBG_MSG("Cannot open GPIO direction it\n");
		 return;
	 }
	 write(directionfd, "out", 4);
	 close(directionfd);
	 valuefd = open("/sys/class/gpio/gpio960/value", O_RDWR);
	 if (valuefd < 0)
	 {
		 DBG_MSG("Cannot open GPIO value\n");
		 return;
	 }
	 if(val == 1)
		 write(valuefd,"1", 2);
	 else if(val == 0)
		 write(valuefd,"0", 2);
	 else
		 DBG_MSG("Invalid parameter, please input 1 for open, 0 for close!");
	 close(valuefd);
}

/* UDP发送函数 */
void *thread_udp_s()
{
	struct can_filter rfilter;//过滤器

/* UDP初始化参数设置*/
    bzero(&serv_addr, len);
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(PORT);
    serv_addr.sin_addr.s_addr =  inet_addr("192.168.137.10");  //"192.168.137.10"为Tx2i端ip
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    if (sockfd < 0)
    {
        DBG_MSG("create socket failed! \n");
        exit(1);
    }

/* UDP接收端阻塞模式设置，阻塞时间0.5s*/
    struct timeval timeOut;
    timeOut.tv_sec = 10;
    timeOut.tv_usec = 0;
    if(setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, &timeOut, sizeof(timeOut)) < 0)
		DBG_MSG("time out setting failed\n");
    else
	{
	   udp_ready = 66;//UDP收数据标志变量
	   DBG_MSG("UDP setting finished!\n");
	}


    while (1)
    {
    	/* 间接指令内容解析，0x01自留，0x02发送TX2 */
        if( indirect_end == 66 )
    	{
    		if( indirect_data[1] == 0x01 )
    		{
    			switch ( indirect_data[2] )
				{
    			            /* count变量手动清零*/
				   case 0:  count_dg = 0;
					        count_pcm = 0;
				            count_gtxsd = 0;
				            count_udpsd = 0;
				            count_sar = 0;
				            DBG_MSG("Clear----count----flag! !\n");
				   		    break;

				   		    /* TX2开机*/
    			   case 1:  ctl_tx2(1);
    			            tx2_state = 0;
							break;

							/* TX2关机*/
				   case 2:  ctl_tx2(0);
				   	   	    tx2_state = (1 << 2);//(B7~B0 = 0000 0100)
				   	   	    memset(tx2_yc, 170, 8);
							break;

							/* TX2重启*/
				   case 3:  ctl_tx2(0);
				            tx2_state = (1 << 2);//(B7~B0 = 0000 0100)
				            memset(tx2_yc, 170, 8);
				            usleep(100*1000);
				            ctl_tx2(1);
				            tx2_state = 0;
							break;

							/* RS422切换至A通道*/
				   case 4:  change_422 = 0;
//				            test_pcm = 0;
				            DBG_MSG("PCM----A RECEIVE DATA !\n");
				            break;

				            /* RS422切换至B通道*/
				   case 5:  change_422 = 1;
//				            test_pcm_1 = 0;
				            DBG_MSG("PCM----B RECEIVE DATA !\n");
				   		    break;

				            /* AI超时时长设定*/
				   case 6:  timeout_flag = 0;
					        alarm(indirect_data[3]*60);//定时xx分钟;
				            DBG_MSG("AI timer is set !\n");
				   		    break;

				   		    /* AI进入数据传输模式（图像回传、重构数据上注）*/
				   case 7:  mode_flag = (1 << 4);//(B7~B0 = 0001 0000)
				            /* 清除过滤设置，下面会重新设置*/
				            setsockopt(fd_a, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
				            setsockopt(fd_b, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
				            /* Filter重新设置  */
							rfilter.can_id = 0x4e2;
							rfilter.can_mask = 0x1f8;
							setsockopt(fd_a, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
							setsockopt(fd_b, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
//							test_id = 0;
//							test_id_1 = 0;
							break;

					   		/* AI进入正常工作模式*/
				   case 8:  mode_flag = 0;
							/* 清除过滤设置，下面会重新设置*/
							setsockopt(fd_a, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
							setsockopt(fd_b, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
				            /* Filter重新设置  */
							rfilter.can_id = 0x4e2;
							rfilter.can_mask = 0x68;
							setsockopt(fd_a, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
							setsockopt(fd_b, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
//							test_id = 0;
//							test_id_1 = 0;
							break;

				   default: break;
				}
    		}
    		/* 间接指令发送至TX2*/
    		else if( indirect_data[1] == 0x02 )
    		{
    			package_indiret();
				if( sendto(sockfd, indirect_send, INDIRECT_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
					fprintf(stderr, "send error: %s\n", strerror(errno));
    		}
            indirect_end = 0;
    	}

        if( mode_flag == 0 )
        {
			/* udp发送GPS广播内容  */
			if( gps_end == 66 )
			{
				if( gps_recvdlc == gps_length + 7 )
				{
					DBG_MSG("GPS ------ SUM : %02x \n", gps_sendsum());
					DBG_MSG("GPS ------ gps_recvdata[sum] : %02x \n", gps_recvdata[gps_recvdlc-1]);
					if( gps_recvdata[gps_recvdlc-1] == gps_sendsum() )
					{
						package_gps();
						if( sendto(sockfd, gps_send, GPS_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
							fprintf(stderr, "send error: %s\n", strerror(errno));

						DBG_MSG("GPS -- GPS -- GPS !!!\n");
					}
				}
				gps_end = 0;
			}

			/* udp发送BDS广播内容 */
			if( bds_end == 66 )
			{
				if( bds_recvdlc == bds_length + 7 )
				{
					DBG_MSG("BDS ------ SUM : %02x \n", bds_sendsum());
					DBG_MSG("BDS ------ bds_recvdata[sum] : %02x \n", bds_recvdata[bds_recvdlc-1]);
					if( bds_recvdata[bds_recvdlc-1] == bds_sendsum() )
					{
						package_bds();
						if( sendto(sockfd, bds_send, BDS_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
							fprintf(stderr, "send error: %s\n", strerror(errno));

						DBG_MSG("BDS -- BDS -- BDS\n");

					}
				}
				bds_end = 0;
			}

			/* udp发送ACS星敏广播内容 */
//	        if( acs_end == 66 )
//	    	{
//	        	if( acs_recvdlc == acs_length + 7 )
//	        	{
//	        		DBG_MSG("ACS ------ SUM : %02x \n", acs_sendsum());
//	        		DBG_MSG("ACS ------ acs_recvdata[sum] : %02x \n", acs_recvdata[acs_recvdlc-1]);
//	        		if( acs_recvdata[acs_recvdlc-1] == acs_sendsum() )
//					{
//	        			package_acs();
//	                    if( sendto(sockfd, acs_send, ACS_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
//	                        fprintf(stderr, "send error: %s\n", strerror(errno));
//	                    DBG_MSG("ACS -- ACS -- ACS\n");
//
//	                    /*地面测试*/
//	                	if( acs_txt==66 )//CAN0
//	                		acsdata_write(fileacs1, acs_recvdata, 252);
//	                	else
//	                		acsdata_write(fileacs2, acs_recvdata, 252);
//					}
//	        	}
//	        	acs_end = 0;
//	    	}

			/* udp发送ACS轨道广播内容 */
//			if( orbit_end == 66 )
//			{
//				if( orbit_recvdlc == orbit_length + 7 )
//				{
//					DBG_MSG("ACS_orbit------ SUM : %02x \n", orbit_sendsum());
//					DBG_MSG("ACS_orbit ------ orbit_recvdata[sum] : %02x \n", orbit_recvdata[orbit_recvdlc-1]);
//					if( orbit_recvdata[orbit_recvdlc-1] == orbit_sendsum() )
//					{
//						package_acsorbit();
//						if( sendto(sockfd, acsorbit_send, ACS_ORBIT_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
//							fprintf(stderr, "send error: %s\n", strerror(errno));
//						DBG_MSG("ACS -- ORBIT -- ACS\n");
//					}
//				}
//				orbit_end = 0;
//			}

			/* udp发送GNSS星历GPS广播内容 */
			if( tlegps_end == 66 )
			{
				if( tlegps_recvdlc == tlegps_length + 7 )
				{
					DBG_MSG("TLE ------ SUM : %02x \n", tlegps_sendsum());
					DBG_MSG("TLE ------ tlegps_recvdata[sum] : %02x \n", tlegps_recvdata[tlegps_recvdlc-1]);
					if( tlegps_recvdata[tlegps_recvdlc-1] == tlegps_sendsum() )
					{
						package_tlegps();
						if( sendto(sockfd, tlegps_send, TLE_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
							fprintf(stderr, "send error: %s\n", strerror(errno));

						DBG_MSG("TLE -- GPS -- GPS -- GPS\n");
					}
				}
				tlegps_end = 0;
			}

			/* udp发送GNSS星历BDS广播内容 */
			if( tlebds_end == 66 )
			{
				if( tlebds_recvdlc == tlebds_length + 7 )
				{
					DBG_MSG("TLE ------ SUM : %02x \n", tlebds_sendsum());
					DBG_MSG("TLE ------ tlebds_recvdata[sum] : %02x \n", tlebds_recvdata[tlebds_recvdlc-1]);
					if( tlebds_recvdata[tlebds_recvdlc-1] == tlebds_sendsum() )
					{
						package_tlebds();
						if( sendto(sockfd, tlebds_send, TLE_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
							fprintf(stderr, "send error: %s\n", strerror(errno));

						DBG_MSG("TLE -- BDS -- BDS -- BDS\n");
					}
				}
				tlebds_end = 0;
			}

			/* udp发送精密定轨结果轮询指令 */
			if( flag_orbit == 66 )
			{
				package_orbit();
				if( sendto(sockfd, orbit_send, ORBIT_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
					fprintf(stderr, "send error: %s\n", strerror(errno));
				flag_orbit = 0;
			}
        }

        /* udp发送RS422通道A数据*/
    	if( dma422_end == 1 )
    	{
    		package_dma422();//999字节
			if( sendto(sockfd, dma422_send, UDP422_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
				fprintf(stderr, "send error: %s\n", strerror(errno));
			else
			{
				if( count_udpsd < 65535 )
					count_udpsd ++;
				else
					count_udpsd = 1;
			}
			dma422_end = 0;//udp数据0.5s内接收应答
    	}
        /* udp发送RS422通道B数据 */
    	else if( dma422_end == 2 )
    	{
			package_dma422_1();//999字节
			if( sendto(sockfd, dma422_send, UDP422_NUM, 0, (const struct sockaddr *)&serv_addr, len) < 0 )
				fprintf(stderr, "send error: %s\n", strerror(errno));
			else
			{
				if( count_udpsd < 65535 )
					count_udpsd ++;
				else
					count_udpsd = 1;
			}
			dma422_end = 0;//udp数据0.5s内接收应答
    	}
    }
    close(sockfd);
    pthread_exit(0);
}

/* UDP监听数据 */
void *thread_udp_r()
{
	int i, ret;
	unsigned char sar_sum = 0;

	tx2_sar[0]=0x5c;//GTX帧头
	tx2_sar[1]=0xfb;
	tx2_sar[994]=0xfd;//GTX帧尾
	tx2_sar[995]=0xfe;

	while( udp_ready != 66 )
	{
	   usleep(100);
	}
	DBG_MSG("udp receive ready! \n");



    while (1)
    {
    	/* udp监听精密定轨数据6+XX+1, ret表示接收到数据大小 */
    	ret = recvfrom(sockfd, udp_recv, UDP_NUM, 0, (struct sockaddr *)&serv_addr, (socklen_t *)&len);

        if (ret > 0)
        {
        	//测试变量
//        	if( ret == 999 )
//        	    count_sarss ++;

        	if( udp_recv[0] == 0x02 )
        	{
        		/* 返回精密定轨62字节数据包 */
                if( udp_recv[5] == 0x08 )
        		{
        			if( udp_recv[68] == udp_recvsum(0x08) )
        			{
        				memcpy(senddata_gb+7, udp_recv+6, 62);//memcpy（目标地址，源地址，数据长度）
        				if( count_dg < 65535 )
							count_dg ++;
						else
							count_dg = 1;

        				/* 打印精密定轨数据，6字节信息体+62字节有效数据*/
//        				printf("222222Orbit Recv_Message :\n");
//        	        	for( i=0; i < 62; i++ )
//        	        	{
//							if(((i%16) == 0)&&(i>0))
//								printf("\n");
//        	        		printf("%02x ", udp_recv[i+6]);
//        	        	}
//        	        	printf("\n");

        			}
        		}
                /* 返回应答帧 */
        		else if( udp_recv[5] == 0x09 )
        		{
        			/* 间接指令*/
        			if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x02) )
        			{
               			if( udp_recv[10] == udp_recvsum(0x09) )
						{
               				if( count_jj < 255 )
               					count_jj ++;
               				else
               					count_jj = 1;
						}
               			DBG_MSG("UDP receive --- count_jj = %d ! \n", count_jj);
        			}
        			/* RS422高速上行，tx2收齐重构数据后回复一帧*/
        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x08) )
        			{
               			if( udp_recv[10] == udp_recvsum(0x09) )
						{
               				if( count_rs < 255 )
               					count_rs ++;
               				else
               					count_rs = 1;
						}
               			DBG_MSG("UDP receive --- count_rs422 = %d ! \n", count_rs);
        			}
        			/* GPS*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x03) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_gps < 65535 )
//               					count_gps ++;
//               				else
//               					count_gps = 1;
//						}
//               			DBG_MSG("UDP receive --- count_gps = %d ! \n", count_gps);
//        			}
        			/* BDS*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x04) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_bds < 65535 )
//               					count_bds ++;
//               				else
//               					count_bds = 1;
//						}
//               			DBG_MSG("UDP receive --- count_bds = %d ! \n", count_bds);
//        			}

        			/* ACS*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x05) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_acs < 65535 )
//               					count_acs ++;
//               				else
//               					count_acs = 1;
//						}
//               			DBG_MSG("UDP receive --- count_acs = %d ! \n", count_acs);
//        			}

        			/* ACS_orbit*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x09) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_orbit < 65535 )
//               					count_orbit ++;
//               				else
//               					count_orbit = 1;
//						}
//               			DBG_MSG("UDP receive --- count_orbit = %d ! \n", count_orbit);
//        			}
        			/* GNSS星历GPS*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x06) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_tlegps < 65535 )
//               					count_tlegps ++;
//               				else
//               					count_tlegps = 1;
//						}
//               			DBG_MSG("UDP receive --- count_tlegps = %d ! \n", count_tlegps);
//        			}
        			/* GNSS星历BDS*/
//        			else if( (udp_recv[6] == 0xB2) && (udp_recv[7] == 0x07) )
//        			{
//               			if( udp_recv[10] == udp_recvsum(0x09) )
//						{
//               				if( count_tlebds < 65535 )
//               					count_tlebds ++;
//               				else
//               					count_tlebds = 1;
//						}
//               			DBG_MSG("UDP receive --- count_tlebds = %d ! \n", count_tlebds);
//        			}
        		}
                /* TX2回传载荷数据  */
        		else if( udp_recv[5] == 0x0A )
        		{
        			//测试SAR图像数据转发执行时间
//        			if( test_flag < 5 )
//        			{
//            			time_1 = get_system_time_microsecond();
//        			}
//        			count_sars++;
//                    memcpy(tx2_sar+2, udp_recv+6, TX2_SAR);//memcpy（目标地址，源地址，数据长度992字节）
//					/* SAR图像GTX接口发送*/
//					if( axidma0send_sar(axidma_dev, &trans_gtx, tx2_sar) >= 0 )
//					{
//						/* SAR图像数据GTX发送计数*/
//						if( count_gtxsd < 16777215 )
//							count_gtxsd ++;
//						else
//							count_gtxsd = 1;
//					}
//					if( count_sar < 16777215 )
//						count_sar ++;
//					else
//						count_sar = 1;





//					if( udp_recv[998] == udp_recvsum(0x0A) )
//					{
						memcpy(tx2_sar+2, udp_recv+6, TX2_SAR);//memcpy（目标地址，源地址，数据长度992字节）

						/* SAR图像数据UDP接收计数*/
						if( count_sar < 16777215 )
							count_sar ++;
						else
							count_sar = 1;

						/* SAR图像GTX接口发送*/
						if( axidma0send(axidma_dev, &trans_gtx, tx2_sar) >= 0 )
						{
							/* SAR图像数据GTX发送计数*/
							if( count_gtxsd < 16777215 )
								count_gtxsd ++;
							else
								count_gtxsd = 1;
						}

						DBG_MSG("UDP receive --- count_sar count_gtxsd = %d %d! \n", count_sar，count_gtxsd);
//					}






					//测试SAR图像数据转发执行时间
//					if( test_flag < 5 )
//					{
//						time_2 = get_system_time_microsecond();
//						test_flag++;
//						printf("time_1 time_2 : %ld  %ld \n",time_1, time_2);
//					}

        		}
                /* TX2返回10字节遥测参数  */
        		else if( udp_recv[5] == 0x0B )
        		{
					if( udp_recv[16] == udp_recvsum(0x0B) )
					{
						memcpy(tx2_yc, udp_recv+6, 8);//memcpy（目标地址，源地址，数据长度）
					}
        		}
        	}


            /* 测试打印收到的UDP数据 */
//        	if( ret == 999 && test_flag1 < 20 )
//        	{
//    			printf("sar_sum: %x \t",sar_sum);
//        		printf("Recvudp message finished: %d !\n", ret);
//				for( i=0; i < ret; i++ )
//				{
//					printf("%02x\t", udp_recv[i]);
//				}
//				printf("\n");
//    			test_flag1++;
//        	}

        }
    }
}




